classdef SLIPoptim < SLIPcycles
    %UNTITLED Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
    end
    
    methods
        
        function obj=matpool(obj)
            try
                a=parcluster;
                obj.sim.mpool=a.NumWorkers;
            catch
                try
                    schd = findResource('scheduler', ...
                        'configuration', defaultParallelConfig);
                    obj.sim.mpool=schd.ClusterSize;
                catch
                    disp('Unable to identify cluster size');
                    obj.sim.mpool=1;
                end
            end
        end
        
        function res_all = walk_eqgait_asym_energy(obj)
            obj.param.E=825;
            res_all=[];
            try
                obj=matpool(obj);
                matlabpool('open',obj.sim.mpool);
            end
            %min_yapex=fsolve(@(y) obj.param.E-obj.param.m*obj.param.g*y-obj.param.k/2*(obj.param.l0-y).^2,0.85);
            y0_all=0.9:0.0002:0.95;
            v_all=sqrt(2/obj.param.m*(obj.param.E-obj.param.m*obj.param.g*y0_all-obj.param.k/2*(obj.param.l0-y0_all).^2));
            phi_v=-0.79:0.005:0.79;
            for idx=1:length(y0_all)
                v=v_all(idx);
                y=y0_all(idx);
                parfor jdx=1:length(phi_v)
                    A=SLIPcycles;
                    A=setIC(A,[0 v*sin(phi_v(jdx)) y v*cos(phi_v(jdx))]);
                    aTD=55:0.1:80;
                    minimum=1;
                    X1=walk_ap2ap_optim(A,aTD(1));
                    for kdx=2:251
                        X2=walk_ap2ap_optim(A,aTD(kdx));
                        if X2<X1
                            minimum=kdx;
                            X1=X2;
                        end
                    end
                    [aTD,X,exitflag]=fminbnd(@(aTD) walk_ap2ap_optim(A,aTD),aTD(minimum)-0.1,aTD(minimum)+0.1);
                    if exitflag==1 && ~A.sim.error && X<1
                        res(jdx,:)=[aTD X v y phi_v(jdx)];
                    end
                    %disp(['y0: ',num2str(y),' v0: ',num2str(v),' phi_v: ',num2str(rad2deg(phi_v(jdx))),' aTD: ',num2str(aTD),' X: ',num2str(X),' exitf: ',num2str(exitflag)]);
                end
                if exist('res')
                    res_all=[res_all;res(res(:,1)~=0,:)];
                    disp(num2str(res_all));
                    clearvars res
                    save res res_all
                end
            end
            try
                matlabpool close
            end
        end
    end
end


